// This file is part of Sophus.
//
// Copyright 2012-2013 Hauke Strasdat
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#ifndef SOPHUS_SIM3_HPP
#define SOPHUS_SIM3_HPP

#include "rxso3.hpp"

////////////////////////////////////////////////////////////////////////////
// Forward Declarations / typedefs
////////////////////////////////////////////////////////////////////////////

namespace Sophus {
	template<typename _Scalar, int _Options=0> class Sim3Group;
	typedef Sim3Group<double> Sim3 EIGEN_DEPRECATED;
	typedef Sim3Group<double> Sim3d; /**< double precision Sim3 */
	typedef Sim3Group<float> Sim3f;  /**< single precision Sim3 */
	typedef Matrix<double,7,1> Vector7d;
	typedef Matrix<double,7,7> Matrix7d;
	typedef Matrix<float,7,1> Vector7f;
	typedef Matrix<float,7,7> Matrix7f;
}

////////////////////////////////////////////////////////////////////////////
// Eigen Traits (For querying derived types in CRTP hierarchy)
////////////////////////////////////////////////////////////////////////////

namespace Eigen {
	namespace internal {
		
		template<typename _Scalar, int _Options>
		struct traits<Sophus::Sim3Group<_Scalar,_Options> > {
			typedef _Scalar Scalar;
			typedef Matrix<Scalar,3,1> TranslationType;
			typedef Sophus::RxSO3Group<Scalar> RxSO3Type;
		};
		
		template<typename _Scalar, int _Options>
		struct traits<Map<Sophus::Sim3Group<_Scalar>, _Options> >
		: traits<Sophus::Sim3Group<_Scalar, _Options> > {
			typedef _Scalar Scalar;
			typedef Map<Matrix<Scalar,3,1>,_Options> TranslationType;
			typedef Map<Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
		};
		
		template<typename _Scalar, int _Options>
		struct traits<Map<const Sophus::Sim3Group<_Scalar>, _Options> >
		: traits<const Sophus::Sim3Group<_Scalar, _Options> > {
			typedef _Scalar Scalar;
			typedef Map<const Matrix<Scalar,3,1>,_Options> TranslationType;
			typedef Map<const Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
		};
		
	}
}

namespace Sophus {
	using namespace Eigen;
	using namespace std;
	
	/**
	 * \brief Sim3 base type - implements Sim3 class but is storage agnostic
	 *
	 * [add more detailed description/tutorial]
	 */
	template<typename Derived>
	class Sim3GroupBase {
	public:
		/** \brief scalar type */
		typedef typename internal::traits<Derived>::Scalar Scalar;
		/** \brief translation reference type */
		typedef typename internal::traits<Derived>::TranslationType &
		TranslationReference;
		/** \brief translation const reference type */
		typedef const typename internal::traits<Derived>::TranslationType &
		ConstTranslationReference;
		/** \brief RxSO3 reference type */
		typedef typename internal::traits<Derived>::RxSO3Type &
		RxSO3Reference;
		/** \brief RxSO3 const reference type */
		typedef const typename internal::traits<Derived>::RxSO3Type &
		ConstRxSO3Reference;
		
		
		/** \brief degree of freedom of group
		 *        (three for translation, three for rotation, one for scale) */
		static const int DoF = 7;
		/** \brief number of internal parameters used
		 *         (quaternion for rotation and scale + translation 3-vector) */
		static const int num_parameters = 7;
		/** \brief group transformations are NxN matrices */
		static const int N = 4;
		/** \brief group transfomation type */
		typedef Matrix<Scalar,N,N> Transformation;
		/** \brief point type */
		typedef Matrix<Scalar,3,1> Point;
		/** \brief tangent vector type */
		typedef Matrix<Scalar,DoF,1> Tangent;
		/** \brief adjoint transformation type */
		typedef Matrix<Scalar,DoF,DoF> Adjoint;
		
		/**
		 * \brief Adjoint transformation
		 *
		 * This function return the adjoint transformation \f$ Ad \f$ of the
		 * group instance \f$ A \f$  such that for all \f$ x \f$
		 * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
		 * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
		 */
		inline
		const Adjoint Adj() const {
			const Matrix<Scalar,3,3> & R = rxso3().rotationMatrix();
			Adjoint res;
			res.setZero();
			res.block(0,0,3,3) = scale()*R;
			res.block(0,3,3,3) = SO3Group<Scalar>::hat(translation())*R;
			res.block(0,6,3,1) = -translation();
			res.block(3,3,3,3) = R;
			res(6,6) = 1;
			return res;
		}
		
		/**
		 * \returns copy of instance casted to NewScalarType
		 */
		template<typename NewScalarType>
		inline Sim3Group<NewScalarType> cast() const {
			return
			Sim3Group<NewScalarType>(rxso3().template cast<NewScalarType>(),
									 translation().template cast<NewScalarType>() );
		}
		
		/**
		 * \brief In-place group multiplication
		 *
		 * Same as operator*=() for Sim3.
		 *
		 * \see operator*()
		 */
		inline
		void fastMultiply(const Sim3Group<Scalar>& other) {
			translation() += (rxso3() * other.translation());
			rxso3() *= other.rxso3();
		}
		
		/**
		 * \returns Group inverse of instance
		 */
		inline
		const Sim3Group<Scalar> inverse() const {
			const RxSO3Group<Scalar> invR = rxso3().inverse();
			return Sim3Group<Scalar>(invR, invR*(translation()
			*static_cast<Scalar>(-1) ) );
		}
		
		/**
		 * \brief Logarithmic map
		 *
		 * \returns tangent space representation
		 *          (translational part and rotation vector) of instance
		 *
		 * \see  log().
		 */
		inline
		const Tangent log() const {
			return log(*this);
		}
		
		/**
		 * \returns 4x4 matrix representation of instance
		 */
		inline
		const Transformation matrix() const {
			Transformation homogenious_matrix;
			homogenious_matrix.setIdentity();
			homogenious_matrix.block(0,0,3,3) = rxso3().matrix();
			homogenious_matrix.col(3).head(3) = translation();
			return homogenious_matrix;
		}
		
		/**
		 * \returns 3x4 matrix representation of instance
		 *
		 * It returns the three first row of matrix().
		 */
		inline
		const Matrix<Scalar,3,4> matrix3x4() const {
			Matrix<Scalar,3,4> matrix;
			matrix.block(0,0,3,3) = rxso3().matrix();
			matrix.col(3) = translation();
			return matrix;
		}
		
		/**
		 * \brief Assignment operator
		 */
		template<typename OtherDerived> inline
		Sim3GroupBase<Derived>& operator=
		(const Sim3GroupBase<OtherDerived> & other) {
			rxso3() = other.rxso3();
			translation() = other.translation();
			return *this;
		}
		
		/**
		 * \brief Group multiplication
		 * \see operator*=()
		 */
		inline
		const Sim3Group<Scalar> operator*(const Sim3Group<Scalar>& other) const {
			Sim3Group<Scalar> result(*this);
			result *= other;
			return result;
		}
		
		/**
		 * \brief Group action on \f$ \mathbf{R}^3 \f$
		 *
		 * \param p point \f$p \in \mathbf{R}^3 \f$
		 * \returns point \f$p' \in \mathbf{R}^3 \f$,
		 *          rotated, scaled and translated version of \f$p\f$
		 *
		 * This function scales, rotates and translates point \f$ p \f$
		 * in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$
		 * (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$.
		 */
		inline
		const Point operator*(const Point & p) const {
			return rxso3()*p + translation();
		}
		
		/**
		 * \brief In-place group multiplication
		 *
		 * \see operator*()
		 */
		inline
		void operator*=(const Sim3Group<Scalar>& other) {
			translation() += (rxso3() * other.translation());
			rxso3() *= other.rxso3();
		}
		
		/**
		 * \brief Mutator of quaternion
		 */
		inline
		typename internal::traits<Derived>::RxSO3Type::QuaternionReference
		quaternion() {
			return rxso3().quaternion();
		}
		
		/**
		 * \brief Accessor of quaternion
		 */
		inline
		typename internal::traits<Derived>::RxSO3Type::ConstQuaternionReference
		quaternion() const {
			return rxso3().quaternion();
		}
		
		/**
		 * \returns Rotation matrix
		 *
		 * deprecated: use rotationMatrix() instead.
		 */
		inline
		EIGEN_DEPRECATED const Transformation rotation_matrix() const {
			return rxso3().rotationMatrix();
		}
		
		/**
		 * \returns Rotation matrix
		 */
		inline
		const Matrix<Scalar,3,3> rotationMatrix() const {
			return rxso3().rotationMatrix();
		}
		
		/**
		 * \brief Mutator of RxSO3 group
		 */
		EIGEN_STRONG_INLINE
		RxSO3Reference rxso3() {
			return static_cast<Derived*>(this)->rxso3();
		}
		
		/**
		 * \brief Accessor of RxSO3 group
		 */
		EIGEN_STRONG_INLINE
		ConstRxSO3Reference rxso3() const {
			return static_cast<const Derived*>(this)->rxso3();
		}
		
		/**
		 * \returns scale
		 */
		EIGEN_STRONG_INLINE
		const Scalar scale() const {
			return rxso3().scale();
		}
		
		/**
		 * \brief Setter of quaternion using rotation matrix, leaves scale untouched
		 *
		 * \param R a 3x3 rotation matrix
		 * \pre       the 3x3 matrix should be orthogonal and have a determinant of 1
		 */
		inline
		void setRotationMatrix
		(const Matrix<Scalar,3,3> & R) {
			rxso3().setRotationMatrix(R);
		}
		
		/**
		 * \brief Scale setter
		 */
		EIGEN_STRONG_INLINE
		void setScale(const Scalar & scale) const {
			rxso3().setScale(scale);
		}
		
		/**
		 * \brief Setter of quaternion using scaled rotation matrix
		 *
		 * \param sR a 3x3 scaled rotation matrix
		 * \pre        the 3x3 matrix should be "scaled orthogonal"
		 *             and have a positive determinant
		 */
		inline
		void setScaledRotationMatrix
		(const Matrix<Scalar,3,3> & sR) {
			rxso3().setScaledRotationMatrix(sR);
		}
		
		/**
		 * \brief Mutator of translation vector
		 */
		EIGEN_STRONG_INLINE
		TranslationReference translation() {
			return static_cast<Derived*>(this)->translation();
		}
		
		/**
		 * \brief Accessor of translation vector
		 */
		EIGEN_STRONG_INLINE
		ConstTranslationReference translation() const {
			return static_cast<const Derived*>(this)->translation();
		}
		
		////////////////////////////////////////////////////////////////////////////
		// public static functions
		////////////////////////////////////////////////////////////////////////////
		
		/**
		 * \param   b 7-vector representation of Lie algebra element
		 * \returns   derivative of Lie bracket
		 *
		 * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$
		 * with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3.
		 *
		 * \see lieBracket()
		 */
		inline static
		const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
			const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
			const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
			Scalar sigma2 = b[6];
			
			Adjoint res;
			res.setZero();
			res.template topLeftCorner<3,3>()
			= -SO3::hat(omega2)-sigma2*Matrix3d::Identity();
			res.template block<3,3>(0,3) = -SO3::hat(upsilon2);
			res.template topRightCorner<3,1>() = upsilon2;
			res.template block<3,3>(3,3) = -SO3::hat(omega2);
			return res;
		}
		
		/**
		 * \brief Group exponential
		 *
		 * \param a tangent space element (7-vector)
		 * \returns corresponding element of the group Sim3
		 *
		 * The first three components of \f$ a \f$ represent the translational
		 * part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three
		 * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
		 *
		 * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
		 * with \f$ \exp(\cdot) \f$ being the matrix exponential
		 * and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3.
		 *
		 * \see hat()
		 * \see log()
		 */
		inline static
		const Sim3Group<Scalar> exp(const Tangent & a) {
			const Matrix<Scalar,3,1> & upsilon = a.segment(0,3);
			const Matrix<Scalar,3,1> & omega = a.segment(3,3);
			Scalar sigma = a[6];
			Scalar theta;
			RxSO3Group<Scalar> rxso3
			= RxSO3Group<Scalar>::expAndTheta(a.template tail<4>(), &theta);
			const Matrix<Scalar,3,3> & Omega = SO3Group<Scalar>::hat(omega);
			const Matrix<Scalar,3,3> & W = calcW(theta, sigma, rxso3.scale(), Omega);
			return Sim3Group<Scalar>(rxso3, W*upsilon);
		}
		
		/**
		 * \brief Generators
		 *
		 * \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$
		 * \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3
		 *
		 * The infinitesimal generators of Sim3 are: \f[
		 *        G_0 = \left( \begin{array}{cccc}
		 *                          0&  0&  0&  1\\
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right),
		 *        G_1 = \left( \begin{array}{cccc}
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  1\\
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right),
		 *        G_2 = \left( \begin{array}{cccc}
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  1\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right).
		 *        G_3 = \left( \begin{array}{cccc}
		 *                          0&  0&  0&  0\\
		 *                          0&  0& -1&  0\\
		 *                          0&  1&  0&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right),
		 *        G_4 = \left( \begin{array}{cccc}
		 *                          0&  0&  1&  0\\
		 *                          0&  0&  0&  0\\
		 *                         -1&  0&  0&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right),
		 *        G_5 = \left( \begin{array}{cccc}
		 *                          0& -1&  0&  0\\
		 *                          1&  0&  0&  0\\
		 *                          0&  0&  0&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right),
		 *        G_6 = \left( \begin{array}{cccc}
		 *                          1&  0&  0&  0\\
		 *                          0&  1&  0&  0\\
		 *                          0&  0&  1&  0\\
		 *                          0&  0&  0&  0
		 *                     \end{array} \right).
		 * \f]
		 * \see hat()
		 */
		inline static
		const Transformation generator(int i) {
			if (i<0 || i>6) {
				throw SophusException("i is not in range [0,6].");
			}
			Tangent e;
			e.setZero();
			e[i] = static_cast<Scalar>(1);
			return hat(e);
		}
		
		/**
		 * \brief hat-operator
		 *
		 * \param omega 7-vector representation of Lie algebra element
		 * \returns     4x4-matrix representatin of Lie algebra element
		 *
		 * Formally, the hat-operator of Sim3 is defined
		 * as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4},
		 * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$
		 * with \f$ G_i \f$ being the ith infinitesial generator().
		 *
		 * \see generator()
		 * \see vee()
		 */
		inline static
		const Transformation hat(const Tangent & v) {
			Transformation Omega;
			Omega.template topLeftCorner<3,3>()
			= RxSO3Group<Scalar>::hat(v.template tail<4>());
			Omega.col(3).template head<3>() = v.template head<3>();
			Omega.row(3).setZero();
			return Omega;
		}
		
		/**
		 * \brief Lie bracket
		 *
		 * \param a 7-vector representation of Lie algebra element
		 * \param b 7-vector representation of Lie algebra element
		 * \returns 7-vector representation of Lie algebra element
		 *
		 * It computes the bracket of Sim3. To be more specific, it
		 * computes \f$ [a, b]_{sim3}
		 * := [\widehat{a}, \widehat{b}]^\vee \f$
		 * with \f$ [A,B] = AB-BA \f$ being the matrix
		 * commutator, \f$ \widehat{\cdot} \f$ the
		 * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of Sim3.
		 *
		 * \see hat()
		 * \see vee()
		 */
		inline static
		const Tangent lieBracket(const Tangent & a,
								 const Tangent & b) {
			const Matrix<Scalar,3,1> & upsilon1 = a.template head<3>();
			const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
			const Matrix<Scalar,3,1> & omega1 = a.template segment<3>(3);
			const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
			Scalar sigma1 = a[6];
			Scalar sigma2 = b[6];
			
			Tangent res;
			res.template head<3>() =
			SO3Group<Scalar>::hat(omega1)*upsilon2
			+ SO3Group<Scalar>::hat(upsilon1)*omega2
			+ sigma1*upsilon2 - sigma2*upsilon1;
			res.template segment<3>(3) = omega1.cross(omega2);
			res[6] = static_cast<Scalar>(0);
			
			return res;
								 }
								 
								 /**
								  * \brief Logarithmic map
								  *
								  * \param other element of the group Sim3
								  * \returns     corresponding tangent space element
								  *              (translational part \f$ \upsilon \f$
								  *               and rotation vector \f$ \omega \f$)
								  *
								  * Computes the logarithmic, the inverse of the group exponential.
								  * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
								  * with \f$ \vee(\cdot) \f$ being the matrix logarithm
								  * and \f$ \vee{\cdot} \f$ the vee()-operator of Sim3.
								  *
								  * \see exp()
								  * \see vee()
								  */
								 inline static
								 const Tangent log(const Sim3Group<Scalar> & other) {
									 Tangent res;
									 Scalar theta;
									 const Matrix<Scalar,4,1> & omega_sigma
									 = RxSO3Group<Scalar>::logAndTheta(other.rxso3(), &theta);
									 const Matrix<Scalar,3,1> & omega = omega_sigma.template head<3>();
									 Scalar sigma = omega_sigma[3];
									 const Matrix<Scalar,3,3> & W
									 = calcW(theta, sigma, other.scale(), SO3Group<Scalar>::hat(omega));
									 res.segment(0,3) = W.partialPivLu().solve(other.translation());
									 res.segment(3,3) = omega;
									 res[6] = sigma;
									 return res;
								 }
								 
								 /**
								  * \brief vee-operator
								  *
								  * \param Omega 4x4-matrix representation of Lie algebra element
								  * \returns     7-vector representatin of Lie algebra element
								  *
								  * This is the inverse of the hat()-operator.
								  *
								  * \see hat()
								  */
								 inline static
								 const Tangent vee(const Transformation & Omega) {
									 Tangent upsilon_omega_sigma;
									 upsilon_omega_sigma.template head<3>()
									 = Omega.col(3).template head<3>();
									 upsilon_omega_sigma.template tail<4>()
									 = RxSO3Group<Scalar>::vee(Omega.template topLeftCorner<3,3>());
									 return upsilon_omega_sigma;
								 }
								 
		 private:
			 static
			 Matrix<Scalar,3,3> calcW(const Scalar & theta,
									  const Scalar & sigma,
							 const Scalar & scale,
							 const Matrix<Scalar,3,3> & Omega){
				 static const Matrix<Scalar,3,3> I
				 = Matrix<Scalar,3,3>::Identity();
				 static const Scalar one = static_cast<Scalar>(1.);
				 static const Scalar half = static_cast<Scalar>(1./2.);
				 Matrix<Scalar,3,3> Omega2 = Omega*Omega;
				 
				 Scalar A,B,C;
				 if (std::abs(sigma)<SophusConstants<Scalar>::epsilon()) {
					 C = one;
					 if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
						 A = half;
						 B = static_cast<Scalar>(1./6.);
					 } else {
						 Scalar theta_sq = theta*theta;
						 A = (one-std::cos(theta))/theta_sq;
						 B = (theta-std::sin(theta))/(theta_sq*theta);
					 }
				 } else {
					 C = (scale-one)/sigma;
					 if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
						 Scalar sigma_sq = sigma*sigma;
						 A = ((sigma-one)*scale+one)/sigma_sq;
						 B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma);
					 } else {
						 Scalar theta_sq = theta*theta;
						 Scalar a = scale*std::sin(theta);
						 Scalar b = scale*std::cos(theta);
						 Scalar c = theta_sq+sigma*sigma;
						 A = (a*sigma+ (one-b)*theta)/(theta*c);
						 B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq);
					 }
				 }
				 return A*Omega + B*Omega2 + C*I;
							 }
		 };
		 
		 /**
		  * \brief Sim3 default type - Constructors and default storage for Sim3 Type
		  */
		 template<typename _Scalar, int _Options>
		 class Sim3Group : public Sim3GroupBase<Sim3Group<_Scalar,_Options> > {
			 typedef Sim3GroupBase<Sim3Group<_Scalar,_Options> > Base;
		 public:
			 /** \brief scalar type */
			 typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
			 ::Scalar Scalar;
			 /** \brief RxSO3 reference type */
			 typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
			 ::RxSO3Type & RxSO3Reference;
			 /** \brief RxSO3 const reference type */
			 typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
			 ::RxSO3Type & ConstRxSO3Reference;
			 /** \brief translation reference type */
			 typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
			 ::TranslationType & TranslationReference;
			 /** \brief translation const reference type */
			 typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
			 ::TranslationType & ConstTranslationReference;
			 
			 /** \brief degree of freedom of group */
			 static const int DoF = Base::DoF;
			 /** \brief number of internal parameters used */
			 static const int num_parameters = Base::num_parameters;
			 /** \brief group transformations are NxN matrices */
			 static const int N = Base::N;
			 /** \brief group transfomation type */
			 typedef typename Base::Transformation Transformation;
			 /** \brief point type */
			 typedef typename Base::Point Point;
			 /** \brief tangent vector type */
			 typedef typename Base::Tangent Tangent;
			 /** \brief adjoint transformation type */
			 typedef typename Base::Adjoint Adjoint;
			 
			 
			 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
			 
			 /**
			  * \brief Default constructor
			  *
			  * Initialize Quaternion to identity rotation and translation to zero.
			  */
			 inline
			 Sim3Group()
			 : translation_( Matrix<Scalar,3,1>::Zero() )
			 {
			 }
			 
			 /**
			  * \brief Copy constructor
			  */
			 template<typename OtherDerived> inline
			 Sim3Group(const Sim3GroupBase<OtherDerived> & other)
			 : rxso3_(other.rxso3()), translation_(other.translation()) {
			 }
			 
			 /**
			  * \brief Constructor from RxSO3 and translation vector
			  */
			 template<typename OtherDerived> inline
			 Sim3Group(const RxSO3GroupBase<OtherDerived> & rxso3,
					   const Point & translation)
			 : rxso3_(rxso3), translation_(translation) {
			 }
			 
			 /**
			  * \brief Constructor from quaternion and translation vector
			  *
			  * \pre quaternion must not be zero
			  */
			 inline
			 Sim3Group(const Quaternion<Scalar> & quaternion,
					   const Point & translation)
			 : rxso3_(quaternion), translation_(translation) {
			 }
			 
			 /**
			  * \brief Constructor from 4x4 matrix
			  *
			  * \pre top-left 3x3 sub-matrix need to be "scaled orthogonal"
			  *      with positive determinant of
			  */
			 inline explicit
			 Sim3Group(const Eigen::Matrix<Scalar,4,4>& T)
			 : rxso3_(T.template topLeftCorner<3,3>()),
			 translation_(T.template block<3,1>(0,3)) {
			 }
			 
			 /**
			  * \returns pointer to internal data
			  *
			  * This provides unsafe read/write access to internal data. Sim3 is
			  * represented by a pair of an RxSO3 element (4 parameters) and translation
			  * vector (three parameters).
			  *
			  * Note: The first three Scalars represent the imaginary parts, while the
			  */
			 EIGEN_STRONG_INLINE
			 Scalar* data() {
				 // rxso3_ and translation_ are layed out sequentially with no padding
				 return rxso3_.data();
			 }
			 
			 /**
			  * \returns const pointer to internal data
			  *
			  * Const version of data().
			  */
			 EIGEN_STRONG_INLINE
			 const Scalar* data() const {
				 // rxso3_ and translation_ are layed out sequentially with no padding
				 return rxso3_.data();
			 }
			 
			 /**
			  * \brief Accessor of RxSO3
			  */
			 EIGEN_STRONG_INLINE
			 RxSO3Reference rxso3() {
				 return rxso3_;
			 }
			 
			 /**
			  * \brief Mutator of RxSO3
			  */
			 EIGEN_STRONG_INLINE
			 ConstRxSO3Reference rxso3() const {
				 return rxso3_;
			 }
			 
			 /**
			  * \brief Mutator of translation vector
			  */
			 EIGEN_STRONG_INLINE
			 TranslationReference translation() {
				 return translation_;
			 }
			 
			 /**
			  * \brief Accessor of translation vector
			  */
			 EIGEN_STRONG_INLINE
			 ConstTranslationReference translation() const {
				 return translation_;
			 }
			 
		 protected:
			 Sophus::RxSO3Group<Scalar> rxso3_;
			 Matrix<Scalar,3,1> translation_;
		 };
		 
		 
		 } // end namespace
		 
		 
		 namespace Eigen {
			 /**
			  * \brief Specialisation of Eigen::Map for Sim3GroupBase
			  *
			  * Allows us to wrap Sim3 Objects around POD array
			  * (e.g. external c style quaternion)
			  */
			 template<typename _Scalar, int _Options>
			 class Map<Sophus::Sim3Group<_Scalar>, _Options>
			 : public Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> > {
				 typedef Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> >
				 Base;
				 
			 public:
				 /** \brief scalar type */
				 typedef typename internal::traits<Map>::Scalar Scalar;
				 /** \brief translation reference type */
				 typedef typename internal::traits<Map>::TranslationType &
				 TranslationReference;
				 /** \brief translation const reference type */
				 typedef const typename internal::traits<Map>::TranslationType &
				 ConstTranslationReference;
				 /** \brief RxSO3 reference type */
				 typedef typename internal::traits<Map>::RxSO3Type &
				 RxSO3Reference;
				 /** \brief RxSO3 const reference type */
				 typedef const typename internal::traits<Map>::RxSO3Type &
				 ConstRxSO3Reference;
				 
				 
				 /** \brief degree of freedom of group */
				 static const int DoF = Base::DoF;
				 /** \brief number of internal parameters used */
				 static const int num_parameters = Base::num_parameters;
				 /** \brief group transformations are NxN matrices */
				 static const int N = Base::N;
				 /** \brief group transfomation type */
				 typedef typename Base::Transformation Transformation;
				 /** \brief point type */
				 typedef typename Base::Point Point;
				 /** \brief tangent vector type */
				 typedef typename Base::Tangent Tangent;
				 /** \brief adjoint transformation type */
				 typedef typename Base::Adjoint Adjoint;
				 
				 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
				 using Base::operator*=;
				 using Base::operator*;
				 
				 EIGEN_STRONG_INLINE
				 Map(Scalar* coeffs)
				 : rxso3_(coeffs),
				 translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
				 }
				 
				 /**
				  * \brief Mutator of RxSO3
				  */
				 EIGEN_STRONG_INLINE
				 RxSO3Reference rxso3() {
					 return rxso3_;
				 }
				 
				 /**
				  * \brief Accessor of RxSO3
				  */
				 EIGEN_STRONG_INLINE
				 ConstRxSO3Reference rxso3() const {
					 return rxso3_;
				 }
				 
				 /**
				  * \brief Mutator of translation vector
				  */
				 EIGEN_STRONG_INLINE
				 TranslationReference translation() {
					 return translation_;
				 }
				 
				 /**
				  * \brief Accessor of translation vector
				  */
				 EIGEN_STRONG_INLINE
				 ConstTranslationReference translation() const {
					 return translation_;
				 }
				 
			 protected:
				 Map<Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
				 Map<Matrix<Scalar,3,1>,_Options> translation_;
			 };
			 
			 /**
			  * \brief Specialisation of Eigen::Map for const Sim3GroupBase
			  *
			  * Allows us to wrap Sim3 Objects around POD array
			  * (e.g. external c style quaternion)
			  */
			 template<typename _Scalar, int _Options>
			 class Map<const Sophus::Sim3Group<_Scalar>, _Options>
			 : public Sophus::Sim3GroupBase<
			 Map<const Sophus::Sim3Group<_Scalar>, _Options> > {
				 typedef Sophus::Sim3GroupBase<
				 Map<const Sophus::Sim3Group<_Scalar>, _Options> > Base;
				 
			 public:
				 /** \brief scalar type */
				 typedef typename internal::traits<Map>::Scalar Scalar;
				 /** \brief translation type */
				 typedef const typename internal::traits<Map>::TranslationType &
				 ConstTranslationReference;
				 /** \brief RxSO3 const reference type */
				 typedef const typename internal::traits<Map>::RxSO3Type &
				 ConstRxSO3Reference;
				 
				 /** \brief degree of freedom of group */
				 static const int DoF = Base::DoF;
				 /** \brief number of internal parameters used */
				 static const int num_parameters = Base::num_parameters;
				 /** \brief group transformations are NxN matrices */
				 static const int N = Base::N;
				 /** \brief group transfomation type */
				 typedef typename Base::Transformation Transformation;
				 /** \brief point type */
				 typedef typename Base::Point Point;
				 /** \brief tangent vector type */
				 typedef typename Base::Tangent Tangent;
				 /** \brief adjoint transformation type */
				 typedef typename Base::Adjoint Adjoint;
				 
				 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
				 using Base::operator*=;
				 using Base::operator*;
				 
				 EIGEN_STRONG_INLINE
				 Map(const Scalar* coeffs)
				 : rxso3_(coeffs),
				 translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
				 }
				 
				 EIGEN_STRONG_INLINE
				 Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs)
				 : translation_(trans_coeffs), rxso3_(rot_coeffs){
				 }
				 
				 /**
				  * \brief Accessor of RxSO3
				  */
				 EIGEN_STRONG_INLINE
				 ConstRxSO3Reference rxso3() const {
					 return rxso3_;
				 }
				 
				 /**
				  * \brief Accessor of translation vector
				  */
				 EIGEN_STRONG_INLINE
				 ConstTranslationReference translation() const {
					 return translation_;
				 }
				 
			 protected:
				 const Map<const Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
				 const Map<const Matrix<Scalar,3,1>,_Options> translation_;
			 };
			 
		 }
		 
		 #endif
		 